#include "./motors.h"
#include "../../device/gpio.h"
#include "../analog.h"
#include "../../utils/progress.h"
#include "./sensor.h"

namespace driver {

using namespace device::gpio;
using namespace utils;

enum class Dir : uint8_t {
    CW = 0x01,
    CCW = 0x02,
    LEFT = CW,
    RIGHT = CCW,
    BRAKE = 0x03,
};

using DIR1 = PA<0>;
using DIR2 = PA<1>;

using STIR = PA<5>;
using WATER = PA<6>;

using ICE1 = PA<7>;
using ICE2 = PA<8>;

static void set_dir1(Dir dir) {
    switch (dir) {
        case Dir::CW: 
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::CCW:
            DIR1::clr();
            DIR2::set();
        break;

        default:    
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

static void set_dir2(Dir dir) {
    switch (dir) {
        case Dir::RIGHT:
            ICE1::set();
            ICE2::clr();
        break;

        case Dir::LEFT:
            ICE1::clr();
            ICE2::set();
        break;

        default:
            ICE1::clr();
            ICE2::clr();
        break;
    }
}

Motors::Motors() {

    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;

    config_pp_out<
        DIR1,
        DIR2,
        STIR,
        WATER,
        ICE1,
        ICE2
    >();

    set_dir1(Dir::BRAKE);
    set_dir2(Dir::BRAKE);
}

Error Motors::stir_box_open() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_stir_top()) {
        goto finish;
    }

    set_dir1(Dir::CW);
    STIR::set();

    for (;;) {
        if (sensor->is_stir_top()) {
            break;
        }
        if (prog.is_after(STIR_TIMEOUT)) {
            err = ec::HEAT_STIR_BOX_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_stir_box_motor_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_STIR_BOX_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }

finish:
    set_dir1(Dir::BRAKE);
    STIR::clr();
    return err;
}

Error Motors::stir_box_close() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_stir_bottom()) {
        goto finish;
    }

    set_dir1(Dir::CCW);
    STIR::set();

    for (;;) {
        if (sensor->is_stir_bottom()) {
            break;
        }
        if (prog.is_after(STIR_TIMEOUT)) {
            err = ec::HEAT_STIR_BOX_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_stir_box_motor_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_STIR_BOX_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }

finish:
    set_dir1(Dir::BRAKE);
    STIR::clr();
    return err;
}

Error Motors::water_left() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_water_right()) {
        goto finish;
    }

    set_dir1(Dir::CCW);
    WATER::set();
    for (;;) {
        if (sensor->is_water_right()) {
            break;
        }
        if (prog.is_after(WATER_TIMEOUT)) {
            err = ec::HEAT_WATER_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_water_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_WATER_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }
finish:
    set_dir1(Dir::BRAKE);
    WATER::clr();
    return err;
}

Error Motors::water_right() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_water_left()) {
        goto finish;
    }

    set_dir1(Dir::CW);
    WATER::set();
    for (;;) {
        if (sensor->is_water_left()) {
            break;
        }
        if (prog.is_after(WATER_TIMEOUT)) {
            err = ec::HEAT_WATER_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_water_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_WATER_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }
finish:
    set_dir1(Dir::BRAKE);
    WATER::clr();
    return err;
}

Error Motors::ice_left() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_ice_right()) {
        goto finish;
    }

    set_dir2(Dir::LEFT);
    for (;;) {
        if (sensor->is_ice_right()) {
            break;
        }
        if (prog.is_after(ICE_TIMEOUT)) {
            err = ec::HEAT_ICE_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_ice_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_ICE_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }
finish:
    set_dir2(Dir::BRAKE);
    return err;
}

Error Motors::ice_right() {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_ice_left()) {
        goto finish;
    }

    set_dir2(Dir::RIGHT);
    for (;;) {
        if (sensor->is_ice_left()) {
            break;
        }
        if (prog.is_after(ICE_TIMEOUT)) {
            err = ec::HEAT_ICE_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_ice_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::HEAT_ICE_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
        osDelay(20);
    }
finish:
    set_dir2(Dir::BRAKE);
    return err;
}


}
